#ifndef _INCL_CPILOT
#define _INCL_CPILOT

#include <mrpt/poses/CPose3D.h>
#include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>

using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace std;

class CPilot
{
      public :
      CPilot();
      ~CPilot();

      void piloteWpt(CPose3D pose_m,TPoint3D v_m);
      
      void piloteHuit(CObservation2DRangeScan scan,CPose3D pose_m,TPoint3D v_m);
      
      void estimation(CObservation2DRangeScan scan,CPose3D pose_m);
      
      void generateHuitWpts();
        
       double getPhiC();
       double getThetaC();
       double getPsiC();
          int etape;
       TPoint3D getCurrentWpt();
        double phi_c;
        double theta_c;
        double psi_c;
        int cur_wpt ;
        vector<TPoint3D> waypts;
        
           
         double xth;
         double yth;
         double Vx;
         double Vy;
         double x1;
         double x2;
         double y1;
         double y2;
         double lth;
         int j;
         double deltatt;
         int icentrale;
         double portee;
         double marge;
         double alphath;
         int imin;
         int imax;
         int angle;
         int indice;
         double Xpot;
         
         double L;
         
         double save;
        private : 
        

};

#endif
